/****************************************************************************
 *
 *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
 *   Copyright (c) 2025 H743 Flight Controller Project. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name PX4 nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

#pragma once

#include <stdint.h>
#include <stdbool.h>

namespace px4
{

/**
 * Atomic boolean type for thread-safe flag operations
 * 
 * Uses ARM Cortex-M7 LDREX/STREX instructions for atomic operations
 */
class atomic_bool
{
public:
	atomic_bool() : _value(0) {}
	explicit atomic_bool(bool value) : _value(value ? 1 : 0) {}

	// Load value atomically
	bool load() const
	{
		// Memory barrier
		__asm__ volatile("dmb" ::: "memory");
		return _value != 0;
	}

	// Store value atomically
	void store(bool value)
	{
		_value = value ? 1 : 0;
		// Memory barrier
		__asm__ volatile("dmb" ::: "memory");
	}

	// Exchange value atomically
	bool exchange(bool value)
	{
		uint32_t old_value;
		uint32_t new_value = value ? 1 : 0;
		uint32_t result;

		do {
			// Load exclusive
			__asm__ volatile("ldrex %0, [%1]" : "=r"(old_value) : "r"(&_value));
			// Store exclusive
			__asm__ volatile("strex %0, %2, [%1]" : "=&r"(result) : "r"(&_value), "r"(new_value));
		} while (result != 0);

		// Memory barrier
		__asm__ volatile("dmb" ::: "memory");

		return old_value != 0;
	}

	// Compare and swap
	bool compare_exchange_strong(bool &expected, bool desired)
	{
		uint32_t exp_val = expected ? 1 : 0;
		uint32_t des_val = desired ? 1 : 0;
		uint32_t old_value;
		uint32_t result;

		do {
			// Load exclusive
			__asm__ volatile("ldrex %0, [%1]" : "=r"(old_value) : "r"(&_value));

			if (old_value != exp_val) {
				// Clear exclusive monitor
				__asm__ volatile("clrex" ::: "memory");
				expected = (old_value != 0);
				return false;
			}

			// Store exclusive
			__asm__ volatile("strex %0, %2, [%1]" : "=&r"(result) : "r"(&_value), "r"(des_val));
		} while (result != 0);

		// Memory barrier
		__asm__ volatile("dmb" ::: "memory");

		return true;
	}

	// Operators
	operator bool() const { return load(); }
	atomic_bool &operator=(bool value) { store(value); return *this; }

private:
	volatile uint32_t _value;
};

/**
 * Atomic integer type for thread-safe counter operations
 */
class atomic_int
{
public:
	atomic_int() : _value(0) {}
	explicit atomic_int(int value) : _value(value) {}

	// Load value atomically
	int load() const
	{
		__asm__ volatile("dmb" ::: "memory");
		return _value;
	}

	// Store value atomically
	void store(int value)
	{
		_value = value;
		__asm__ volatile("dmb" ::: "memory");
	}

	// Fetch and add
	int fetch_add(int arg)
	{
		int old_value;
		int new_value;
		uint32_t result;

		do {
			__asm__ volatile("ldrex %0, [%1]" : "=r"(old_value) : "r"(&_value));
			new_value = old_value + arg;
			__asm__ volatile("strex %0, %2, [%1]" : "=&r"(result) : "r"(&_value), "r"(new_value));
		} while (result != 0);

		__asm__ volatile("dmb" ::: "memory");
		return old_value;
	}

	// Operators
	operator int() const { return load(); }
	atomic_int &operator=(int value) { store(value); return *this; }
	int operator++() { return fetch_add(1) + 1; }
	int operator++(int) { return fetch_add(1); }

private:
	volatile int _value;
};

} // namespace px4

